#pragma once



// write by lbf_heu  2023/04/15
// target: 在pangolin中显示仿真系统

#include "head.hpp"
#include "robot.hpp"

#include <pangolin/pangolin.h>


class viewer{
public:
    std::unique_ptr<pangolin::Var<bool>> menu_grid_;
    std::unique_ptr<pangolin::Var<bool>> menu_reset_;
    std::unique_ptr<pangolin::Var<bool>> menu_show_info_;
    std::unique_ptr<pangolin::Var<float>> menu_wall_size_;
    std::unique_ptr<pangolin::Var<bool>> menu_begin_;
    std::mutex mtx_;// 线程锁
    bool beginFlag = false;
    bool resetFlag = false;
public:
    robots* robo;
    // 点云
    std::vector<Eigen::Vector6d> pointcloud;
    // 机器人坐标
    Eigen::Matrix3d mPos = Eigen::Matrix3d::Identity();
    Eigen::Vector3d mTrans = Eigen::Vector3d(0., 0., 0.);

    std::mutex mDataMutex;

    viewer(robots* _robo){
        robo = _robo;
        m2dPath.reserve(100000);
    }
    ~viewer(){}
    // 重载括号运算符号
    void operator() ()
    {
        pangolin::View d_cam;
        pangolin::OpenGlRenderState s_cam;
        pangolin::CreateWindowAndBind("Viewer", 1024, 768);
        glEnable(GL_DEPTH_TEST);
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

        s_cam = pangolin::OpenGlRenderState(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
        );

        d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));
        
        // 创建按钮
        create_menu_panel();

        while(pangolin::ShouldQuit() == false){
            // 每次显示都清空
            glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
            d_cam.Activate(s_cam);
            glClearColor(0.15f, 0.15f, 0.15f, 1.0f);
            
            // 绘制网格
            draw_horizontal_grid();

            mDataMutex.lock();

            showRobot(mPos, mTrans);
            showWalls();// 显示简单墙面
            showLasers(); //显示激光点
            showLasersDir();  //显示激光的方向
            showPathPoint();// 显示优化后的位姿数据
            mDataMutex.unlock();

            // 重启请求
            if(*menu_reset_){
                mtx_.lock();
                resetFlag = true;
                mtx_.unlock();
                *menu_reset_ = false;
            }

            // 开始请求
            if(*menu_begin_){
                mtx_.lock();
                beginFlag = true;
                mtx_.unlock();
                *menu_begin_ = false;
            }

            pangolin::FinishFrame();
            usleep(5000);   // sleep 5 ms
        }
    }
    // 显示机器人
    void showRobot(const Eigen::Matrix3d & _robotPos, const Eigen::Vector3d & _robotTrans){
        std::vector<Eigen::Vector3d> wheelvec;
        robo->getWheelPos(_robotPos, _robotTrans, wheelvec);

        glPointSize(10);
        glBegin(GL_POINTS);
        glColor3d(0., 1., 0.);
        glVertex3d(wheelvec[0][0], wheelvec[0][1], wheelvec[0][2]);
        glEnd();

        glLineWidth(2);

        glColor3f(1.0f, 0.0f, 0.0f);  // blue for ground truth
        glBegin(GL_LINES);
        glVertex3d(wheelvec[0][0], wheelvec[0][1], wheelvec[0][2]);
        glVertex3d(wheelvec[1][0], wheelvec[1][1], wheelvec[1][2]);
        glEnd();

        glBegin(GL_LINES);
        glVertex3d(wheelvec[1][0], wheelvec[1][1], wheelvec[1][2]);
        glVertex3d(wheelvec[2][0], wheelvec[2][1], wheelvec[2][2]);
        glEnd();

        glBegin(GL_LINES);
        glVertex3d(wheelvec[0][0], wheelvec[0][1], wheelvec[0][2]);
        glVertex3d(wheelvec[2][0], wheelvec[2][1], wheelvec[2][2]);
        glEnd();

        Eigen::Vector3d ps = _robotPos * Eigen::Vector3d(0.1, 0., -0.1) + _robotTrans;
        glBegin(GL_LINES);
        glVertex3d(_robotTrans[0], _robotTrans[1], wheelvec[0][2]);
        glVertex3d(ps[0], ps[1], ps[2]);
        glEnd();

        glColor3d(0., 1., 0.);
        ps = _robotPos * Eigen::Vector3d(0., 0.1, -0.1) + _robotTrans;
        glBegin(GL_LINES);
        glVertex3d(_robotTrans[0], _robotTrans[1], wheelvec[0][2]);
        glVertex3d(ps[0], ps[1], ps[2]);
        glEnd();


    }
    // 显示三维点
    void showPointCloud() {

        if (pointcloud.empty()) {
            std::cerr << "Point cloud is empty!" << std::endl;
            return;
        }
        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
    }
    
    void setPoint(std::vector<Eigen::Vector6d> & _pointcloud){
        mDataMutex.lock();
        pointcloud.clear();
        for(int i = 0; i < _pointcloud.size(); i++){
            pointcloud.push_back(_pointcloud[i]);
        }
        mDataMutex.unlock();
    }

    // 设置路径点的轨迹
    std::vector<Eigen::Vector3d> m2dPath;

    void setPathPoint(Eigen::Vector3d path){
        mDataMutex.lock();
        m2dPath.push_back(path);
        mDataMutex.unlock();
    }
    void showPathPoint(){
        glColor3d(0., 1., 0.);
        glPointSize(2);
        glBegin(GL_POINTS);
        for(auto & val : m2dPath){
            glVertex3d(val[0], val[1], 0.);
        }
        glEnd();
    }

    void setPosAndTrans(const Eigen::Matrix3d & _pos, const Eigen::Vector3d & _tran){
        mDataMutex.lock();
        mPos = _pos;
        mTrans = _tran;
        mDataMutex.unlock();
    }
    // 设置显示激光点所在的位置
    std::vector<Eigen::Vector9d> mLasers;
    void setLasers(std::vector<Eigen::Vector9d> lasers){// 设置激光点的位置
        // copy
        mDataMutex.lock();
        mLasers = lasers;
        mDataMutex.unlock();
    }
    
    void showLasers(){

        for(size_t i = 0; i < mLasers.size(); i++){
            // 激光发射器在世界坐标系的位置
            glPointSize(5);
            glBegin(GL_POINTS);
            glColor3d(0., 1., 1.);
            glVertex3d(mLasers[i](6, 0), mLasers[i](7, 0), mLasers[i](8, 0));
            glEnd();
            // 激光照射点在世界坐标系的位置 
            glPointSize(5);
            glBegin(GL_POINTS);
            glColor3d(1., 0., 1.);
            glVertex3d(mLasers[i](0, 0), mLasers[i](1, 0), mLasers[i](2, 0));
            glEnd();
        }
    }
    std::vector<Eigen::Vector6d> mLasersDir;
    void setLasersLoaction(std::vector<Eigen::Vector6d> lasersDir){
        mDataMutex.lock();
        mLasersDir = lasersDir;
        mDataMutex.unlock();
    }

    void showLasersDir(){
        glLineWidth(2);
        for(size_t i = 0; i < mLasersDir.size(); i++){
            glColor3f(1.0f, 0.5f, 0.5f);  // 绿色的是y轴了
            glBegin(GL_LINES);
            glVertex3d(mLasersDir[i](0, 0), mLasersDir[i](1, 0), mLasersDir[i](2, 0));
            glVertex3d(mLasersDir[i](3, 0), mLasersDir[i](4, 0), mLasersDir[i](5, 0));
            glEnd();
        }
    }
    // 显示墙
    std::vector<Eigen::Vector3d> mWalls;
    void setWalls(std::vector<Eigen::Vector3d> _walls){// 设置墙的数

        mWalls = _walls;
        // 显示的墙面在范围内
        std::vector<Eigen::Vector3d> boundaryLines;
        double maxLine = 5.;
        double minLine = 0.;
        boundaryLines.push_back(Eigen::Vector3d(1., 0., -maxLine));
        boundaryLines.push_back(Eigen::Vector3d(0., 1., -maxLine));
        boundaryLines.push_back(Eigen::Vector3d(1., 0., -minLine));
        boundaryLines.push_back(Eigen::Vector3d(0., 1., -minLine));

        for(size_t i = 0; i < mWalls.size(); ++i){
            // 每条线和v边界线求交点
            std::vector<Eigen::Vector3d> intersections;
            for(size_t j = 0; j < boundaryLines.size(); j++){
                Eigen::Vector3d res = mWalls[i].cross(boundaryLines[j]);
                if(res[2] < 0) res = -res;
                std::cout << mWalls[i].transpose() << " × " <<  boundaryLines[j].transpose() << " = " << res.transpose() << std::endl;
                if(abs(res[2]) < 1e-5) continue;// 此时是平行的情况
                if(res[1] >= minLine-0.1 && res[1] <= maxLine+0.1) 
                if(res[0] >= minLine-0.1 && res[0] <= maxLine+0.1) intersections.push_back(res);// 保存点
            }
            // 保存一组端点
            if(intersections.size() >= 2){
                mWallPoints.push_back(Eigen::Vector4d(intersections[0][0], intersections[0][1], intersections[1][0], intersections[1][1]));
            }
            std::cout << mWallPoints.size() << std::endl;
        }
    }
    std::vector<Eigen::Vector4d> mWallPoints;// 存储墙的两个断点

    void showWalls(){// 暂时显示5m是 0-0 伸出去5m
        float widthAdd = *menu_wall_size_;
        glLineWidth(2 * widthAdd);
  
        for(size_t i = 0; i < mWallPoints.size(); ++i){
            if(i == 0) glColor3f(1.0f, 0.0f, 0.0f);  // 红的是x轴了
            if(i == 1) glColor3f(0.0f, 1.0f, 0.0f);  // 绿色的是y轴了
            glBegin(GL_LINES);
            glVertex3d(mWallPoints[i][0], mWallPoints[i][1], 0.);
            glVertex3d(mWallPoints[i][2], mWallPoints[i][3], 0.);
            glEnd();
            // std::cout << "i = " << i << "(" << mWallPoints[i][0] << "," << mWallPoints[i][1] << ")-" << "(" << mWallPoints[i][2] << "," << mWallPoints[i][3] << ")" << std::endl;
        }


    }


    // 创建按钮栏目
    void create_menu_panel()
    {
        // 创建左边的块
        pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(175));
        // 显示二维网格
        menu_grid_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Grid", true, true));
        // 显示更多信息
        menu_show_info_ =  std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Info", false, true));
        menu_wall_size_ = std::unique_ptr<pangolin::Var<float>>(new pangolin::Var<float>("menu.Wall Size", 1.0, 1., 1e1, true));
        // 重置
        menu_reset_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Reset", false, false));
        // 重置
        menu_begin_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Begin", false, false));
    }
    // 绘制地面网格
    void draw_horizontal_grid(void)
    {
        if (!*menu_grid_){
            return;
        }

        Eigen::Matrix4f origin;
        origin << 1, 0, 0, 0, 
                  0, 1, 0, 0, 
                  0, 0, 1, 0, 
                  0, 0, 0, 1;
        glPushMatrix();
        glMultTransposeMatrixf(origin.data());

        glLineWidth(1);
        glColor3f(0.3f, 0.3f, 0.3f);

        glBegin(GL_LINES);

        constexpr float interval_ratio = 0.5;
        constexpr float grid_min = 0;
        constexpr float grid_max = 5.0f;

        for (float x = 0; x <= 5; x += interval_ratio){
            draw_line(x, grid_min, 0, x, grid_max, 0);
        }
        for (float y = 0; y <= 5; y += interval_ratio)
        {
            draw_line(grid_min, y, 0, grid_max, y, 0);
        }

        glEnd();

        glPopMatrix();
    }
    void draw_line(const float x1, const float y1, const float z1, const float x2, const float y2, const float z2) const
    {
        glVertex3f(x1, y1, z1);
        glVertex3f(x2, y2, z2);
    }

    bool getEnableBegin(){
        bool res = false; 
        mtx_.lock();
        res = beginFlag;
        beginFlag = false;// 默认归0
        mtx_.unlock();
        return res;
    }

    bool getEnableReset(){
        bool res = false; 
        mtx_.lock();
        res = resetFlag;
        resetFlag = false;// 默认归0
        mtx_.unlock();
        return res;
    }

    void resetViewer(){
        m2dPath.clear();
    }
};


